%Author:唐友康
%Attention↓
%**仿真动画一次只能仿真一个，在最后仿真，切换仿真动画时请注释当前动画，反注释另一个动画**/
clc;
clear
Ir = 1.19 * 10^(-5);  %转子惯量
Br = 3.10 * 10^(-4);  %转子阻尼
m = 0.5;              %连杆质量
L = 0.1;              %连杆质心距离转轴距离
I1 = 5.0 * 10^(-3);    %负载惯量
B1 = 0.2;             %关节阻尼
Ku = 3;               %电压增益
Kg = 0.55;            %跨导增益
g = 9.8;              %重力加速度
Ra = 2.49;            %电枢电阻
La = 6.1 * 10^(-4);   %电枢电感
Ka = 8.22 * 10^(-2);  %转矩常数
Ke = 8.24 * 10^(-2);  %感应电动势常数
G_sel = 0;            %先不考虑重力

n = 10;               %传动比
uc1 = 4;              %控制电压1
uc2 = 8;              %控制电压2
ua1 = uc1*Ku;         %电枢电压1
ua2 = uc2*Ku;         %电枢电压2

Im = Ir + I1/(n^(2));  %等效惯量
Bm = Br + B1/(n^(2));  %等效阻尼

%**************速度模式**********************************************/
%************** 水平平面 n = 10********************************************/
t = 0:0.0001:0.2;        %仿真时间
w1 = zeros(2001,1);    %角速度1
i1 = zeros(2001,1);    %电流1
theta1 = zeros(2001,1);%角度1
w2 = zeros(2001,1);    %角速度2
i2 = zeros(2001,1);    %电流2
theta2 = zeros(2001,1);%角度2

for i = 1:length(t)-1
    %计算控制电压4v的情况
    [theta1(i+1), i1(i+1), w1(i+1)] = Vel_mode(Ke, w1(i), ua1, Ra, i1(i), La, Ka, Im, Bm, G_sel, m, L, g, theta1(i), n);
    %计算控制电压8v的情况
    [theta2(i+1), i2(i+1), w2(i+1)] = Vel_mode(Ke, w2(i), ua2, Ra, i2(i), La, Ka, Im, Bm, G_sel, m, L, g, theta2(i), n);
end
figure(1);
yyaxis left;
plot(t,w1,'r-',t,w2,'b-');
ylabel('wm(t)(rad/s)');
hold on;
yyaxis right;
plot(t,i1,'r--',t,i2,'b--');
ylim([0,16]);
ylabel('ia(t)(A)');
legend('w, uc = 4v','w, uc = 8v', 'ia, uc = 4v', 'ia, uc = 8v');
title('速度模式，水平平面，传动比10');

%************** 竖直平面 n = 10 ********************************************/
% G_sel = 1;
% t = 0:0.0001:2;        %仿真时间
% w1 = zeros(20001,1);    %角速度1
% i1 = zeros(20001,1);    %电流1
% theta3 = zeros(20001,1);%角度3
% w2 = zeros(20001,1);    %角速度2
% i2 = zeros(20001,1);    %电流2
% theta4 = zeros(20001,1);%角度4
% for i = 1:length(t)-1
%     %计算控制电压4v的情况
%     [theta3(i+1), i1(i+1), w1(i+1)] = Vel_mode(Ke, w1(i), ua1, Ra, i1(i), La, Ka, Im, Bm, G_sel, m, L, g, theta3(i),n);
%     %计算控制电压8v的情况
%     [theta4(i+1), i2(i+1), w2(i+1)] = Vel_mode(Ke, w2(i), ua2, Ra, i2(i), La, Ka, Im, Bm, G_sel, m, L, g, theta4(i),n);
% end
% figure(2);
% yyaxis left;
% plot(t,w1,'r-',t,w2,'b-');
% ylabel('wm(t)(rad/s)');
% hold on;
% yyaxis right;
% plot(t,i1,'r--',t,i2,'b--');
% ylim([0,16]);
% ylabel('ia(t)(A)');
% legend('w, uc = 4v','w, uc = 8v', 'ia, uc = 4v', 'ia, uc = 8v');
% title('速度模式，竖直平面，传动比10');
% 
% %********************竖直平面 n = 50 *************************************************/
% n = 50;
% Im = Ir + I1/(n^(2));  %等效惯量
% Bm = Br + B1/(n^(2));  %等效阻尼
% G_sel = 1;
% t = 0:0.0001:2;        %仿真时间
% w1 = zeros(20001,1);    %角速度1
% i1 = zeros(20001,1);    %电流1
% theta5 = zeros(20001,1);%角度5
% w2 = zeros(20001,1);    %角速度2
% i2 = zeros(20001,1);    %电流2
% theta6 = zeros(20001,1);%角度6
% for i = 1:length(t)-1
%     %计算控制电压4v的情况
%     [theta5(i+1), i1(i+1), w1(i+1)] = Vel_mode(Ke, w1(i), ua1, Ra, i1(i), La, Ka, Im, Bm, G_sel, m, L, g, theta5(i),n);
%     %计算控制电压8v的情况
%     [theta6(i+1), i2(i+1), w2(i+1)] = Vel_mode(Ke, w2(i), ua2, Ra, i2(i), La, Ka, Im, Bm, G_sel, m, L, g, theta6(i),n);
% end
% figure(3);
% yyaxis left;
% plot(t,w1,'r-',t,w2,'b-');
% ylabel('wm(t)(rad/s)');
% hold on;
% yyaxis right;
% plot(t,i1,'r--',t,i2,'b--');
% ylim([0,30]);
% ylabel('ia(t)(A)');
% legend('w, uc = 4v','w, uc = 8v', 'ia, uc = 4v', 'ia, uc = 8v');
% title('速度模式，竖直平面，传动比50');
% 
% %******************力矩模式**********************************************/
% %******************水平平面，传动比10**********************************************/
% G_sel = 0;
% n = 10;
% Im = Ir + I1/(n^(2));  %等效惯量
% Bm = Br + B1/(n^(2));  %等效阻尼
% t = 0:0.0001:0.2;        %仿真时间
% w1 = zeros(2001,1);    %角速度1
% i1 = zeros(2001,1);    %电流1
% theta7 = zeros(2001,1);%角度7
% w2 = zeros(2001,1);    %角速度2
% i2 = zeros(2001,1);    %电流2
% theta8 = zeros(2001,1);%角度8
% for i = 1:length(t)-1
%     %计算控制电压4v的情况
%     [theta7(i+1), i1(i+1), w1(i+1)] = Torque_mode(Kg, w1(i), uc1, Ka, Im, Bm, G_sel, m, L, g, theta7(i),n);
%     %计算控制电压8v的情况
%     [theta8(i+1), i2(i+1), w2(i+1)] = Torque_mode(Kg, w2(i), uc2, Ka, Im, Bm, G_sel, m, L, g, theta8(i),n);
% end
% figure(4);
% yyaxis left;
% plot(t,w1,'r-',t,w2,'b-');
% ylabel('wm(t)(rad/s)');
% hold on;
% yyaxis right;
% plot(t,i1,'r--',t,i2,'b--');
% ylim([0,16]);
% ylabel('ia(t)(A)');
% legend('w, uc = 4v','w, uc = 8v', 'ia, uc = 4v', 'ia, uc = 8v');
% title('力矩模式，水平平面，传动比10');
% 
% %******************* 竖直平面 传动比10 ******************************************/
% G_sel = 1;
% n = 10;
% Im = Ir + I1/(n^(2));  %等效惯量
% Bm = Br + B1/(n^(2));  %等效阻尼
% t = 0:0.0001:2;        %仿真时间
% w1 = zeros(20001,1);    %角速度1
% i1 = zeros(20001,1);    %电流1
% theta9 = zeros(20001,1);%角度9
% w2 = zeros(20001,1);    %角速度2
% i2 = zeros(20001,1);    %电流2
% theta10 = zeros(20001,1);%角度10
% for i = 1:length(t)-1
%     %计算控制电压4v的情况
%     [theta9(i+1), i1(i+1), w1(i+1)] = Torque_mode(Kg, w1(i), uc1, Ka, Im, Bm, G_sel, m, L, g, theta9(i),n);
%     %计算控制电压8v的情况
%     [theta10(i+1), i2(i+1), w2(i+1)] = Torque_mode(Kg, w2(i), uc2, Ka, Im, Bm, G_sel, m, L, g, theta10(i),n);
% end
% figure(5);
% yyaxis left;
% plot(t,w1,'r-',t,w2,'b-');
% ylabel('wm(t)(rad/s)');
% hold on;
% yyaxis right;
% plot(t,i1,'r--',t,i2,'b--');
% ylim([0,16]);
% ylabel('ia(t)(A)');
% legend('w, uc = 4v','w, uc = 8v', 'ia, uc = 4v', 'ia, uc = 8v');
% title('力矩模式，竖直平面，传动比10');
% 
% %******************* 竖直平面 传动比50 ******************************************/
% G_sel = 1;
% n = 50;
% Im = Ir + I1/(n^(2));  %等效惯量
% Bm = Br + B1/(n^(2));  %等效阻尼
% t = 0:0.0001:2;        %仿真时间
% w1 = zeros(20001,1);    %角速度1
% i1 = zeros(20001,1);    %电流1
% theta11 = zeros(20001,1);%角度11
% w2 = zeros(20001,1);    %角速度2
% i2 = zeros(20001,1);    %电流2
% theta12 = zeros(20001,1);%角度12
% for i = 1:length(t)-1
%     %计算控制电压4v的情况
%     [theta11(i+1), i1(i+1), w1(i+1)] = Torque_mode(Kg, w1(i), uc1, Ka, Im, Bm, G_sel, m, L, g, theta11(i),n);
%     %计算控制电压8v的情况
%     [theta12(i+1), i2(i+1), w2(i+1)] = Torque_mode(Kg, w2(i), uc2, Ka, Im, Bm, G_sel, m, L, g, theta12(i),n);
% end
% figure(6);
% yyaxis left;
% plot(t,w1,'r-',t,w2,'b-');
% ylabel('wm(t)(rad/s)');
% hold on;
% yyaxis right;
% plot(t,i1,'r--',t,i2,'b--');
% ylim([0,100]);
% ylabel('ia(t)(A)');
% legend('w, uc = 4v','w, uc = 8v', 'ia, uc = 4v', 'ia, uc = 8v');
% title('力矩模式，竖直平面，传动比50');

%************************* 仿真动画 ********************************************/
%*********速度模式 n = 10 水平平面 uc = 4v
% L1(1) = Link('d',0,'a',0,'alpha',pi/2,'modified');
% L1(2) = Link('d',0,'a',1,'alpha',0,'modified');
% link=SerialLink( L1, 'name', 'R' );
% joint(: ,1)=-theta1;
% joint(: ,2)=0;
% figure(7);
% link.plot(joint ,'jointdiam',1,'fps',100,'trail','b-');

%*********速度模式 n = 10 水平平面 uc = 8v
% L1(1) = Link('d',0,'a',0,'alpha',pi/2,'modified');
% L1(2) = Link('d',0,'a',1,'alpha',0,'modified');
% link=SerialLink( L1, 'name', 'R' );
% joint(: ,1)=-theta2;
% joint(: ,2)=0;
% figure(7);
% link.plot(joint ,'jointdiam',1,'fps',100,'trail','b-');

%*********速度模式 n = 10 竖直平面 uc = 4v
% L1(1) = Link('d',0,'a',0,'alpha',pi/2,'modified');
% L1(2) = Link('d',0,'a',1,'alpha',0,'modified');
% link=SerialLink( L1, 'name', 'R' );
% joint(: ,1)=-theta3;
% joint(: ,2)=0;
% figure(7);
% link.plot(joint ,'jointdiam',1,'fps',100,'trail','b-');

%*********速度模式 n = 10 竖直平面 uc = 8v
% L1(1) = Link('d',0,'a',0,'alpha',pi/2,'modified');
% L1(2) = Link('d',0,'a',1,'alpha',0,'modified');
% link=SerialLink( L1, 'name', 'R' );
% joint(: ,1)=-theta4;
% joint(: ,2)=0;
% figure(7);
% link.plot(joint ,'jointdiam',1,'fps',100,'trail','b-');

%*********速度模式 n = 50 竖直平面 uc = 4v
% L1(1) = Link('d',0,'a',0,'alpha',pi/2,'modified');
% L1(2) = Link('d',0,'a',1,'alpha',0,'modified');
% link=SerialLink( L1, 'name', 'R' );
% joint(: ,1)=-theta5;
% joint(: ,2)=0;
% figure(7);
% link.plot(joint ,'jointdiam',1,'fps',100,'trail','b-');

%*********速度模式 n = 50 竖直平面 uc = 8v
% L1(1) = Link('d',0,'a',0,'alpha',pi/2,'modified');
% L1(2) = Link('d',0,'a',1,'alpha',0,'modified');
% link=SerialLink( L1, 'name', 'R' );
% joint(: ,1)=-theta6;
% joint(: ,2)=0;
% figure(7);
% link.plot(joint ,'jointdiam',1,'fps',100,'trail','b-');

%*********力矩模式 n = 10 水平平面 uc = 4v
% L1(1) = Link('d',0,'a',0,'alpha',pi/2,'modified');
% L1(2) = Link('d',0,'a',1,'alpha',0,'modified');
% link=SerialLink( L1, 'name', 'R' );
% joint(: ,1)=-theta7;
% joint(: ,2)=0;
% figure(7);
% link.plot(joint ,'jointdiam',1,'fps',100,'trail','b-');

%*********力矩模式 n = 10 水平平面 uc = 8v
% L1(1) = Link('d',0,'a',0,'alpha',pi/2,'modified');
% L1(2) = Link('d',0,'a',1,'alpha',0,'modified');
% link=SerialLink( L1, 'name', 'R' );
% joint(: ,1)=-theta8;
% joint(: ,2)=0;
% figure(7);
% link.plot(joint ,'jointdiam',1,'fps',100,'trail','b-');

%*********力矩模式 n = 10 竖直平面 uc = 4v
% L1(1) = Link('d',0,'a',0,'alpha',pi/2,'modified');
% L1(2) = Link('d',0,'a',1,'alpha',0,'modified');
% link=SerialLink( L1, 'name', 'R' );
% joint(: ,1)=-theta9;
% joint(: ,2)=0;
% figure(7);
% link.plot(joint ,'jointdiam',1,'fps',100,'trail','b-');

%*********力矩模式 n = 10 竖直平面 uc = 8v
% L1(1) = Link('d',0,'a',0,'alpha',pi/2,'modified');
% L1(2) = Link('d',0,'a',1,'alpha',0,'modified');
% link=SerialLink( L1, 'name', 'R' );
% joint(: ,1)=-theta10;
% joint(: ,2)=0;
% figure(7);
% link.plot(joint ,'jointdiam',1,'fps',100,'trail','b-');

%*********力矩模式 n = 50 竖直平面 uc = 4v
% L1(1) = Link('d',0,'a',0,'alpha',pi/2,'modified');
% L1(2) = Link('d',0,'a',1,'alpha',0,'modified');
% link=SerialLink( L1, 'name', 'R' );
% joint(: ,1)=-theta11;
% joint(: ,2)=0;
% figure(7);
% link.plot(joint ,'jointdiam',1,'fps',100,'trail','b-');

%********* 力矩模式 n = 50 竖直平面 uc = 8v
% L1(1) = Link('d',0,'a',0,'alpha',pi/2,'modified');
% L1(2) = Link('d',0,'a',1,'alpha',0,'modified');
% link=SerialLink( L1, 'name', 'R' );
% joint(: ,1)=-theta12;
% joint(: ,2)=0;
% figure(7);
% link.plot(joint ,'jointdiam',1,'fps',100,'trail','b-');